#include <Arduino.h>

#define WHEEL_TRACK  (0.33)
#define WHEEL_RADIUS (0.0625)
#define FACTOR_30K   (0.00825)

//左右2个轮子的速度m/s
struct DiffVel {
  long left;
  long right;

  // uint8_t left_dir;
  // uint8_t right_dir;
};

double rpmToValue(double rpm)
{
//  Serial.print("\nrpm ==== ");
//  Serial.print(rpm);  
  return rpm / FACTOR_30K;
}
/*
 * lin 速度 m / s
 * lin * 60 :一分钟走的路程
 * 
*/
double linearToRpm(double lin)
{
  double mileage = lin * 60.0;//mileage in one minute
  double rpm = mileage / (2.0 * M_PI * WHEEL_RADIUS) / 1000.0;// RPM = mileage / (2*pi*radius)
  
  return rpm;
}

void update(const long desiredSpeed, const long desiredRadius, DiffVel & vel)
{
    bool change_direction(false);

    long left, right;

    if( desiredSpeed == 0 && desiredRadius == 0 )
    {
        vel.left = 0;
        vel.right = 0;
        return;
    }

    if( desiredRadius == 0 )
    {
        vel.left   = desiredSpeed;
        vel.right  = desiredSpeed;
    }
    else if( desiredRadius == -1 || desiredRadius == +1 )
    {
        vel.left = -desiredRadius * desiredSpeed;
        vel.right = desiredRadius * desiredSpeed;
    }
    else
    {
        long radius( desiredRadius );
        if( radius < 0 )
        {
            radius = -radius;
            change_direction = true;
        }

        left = ( radius - WHEEL_TRACK / 2.0 * 1000.0);
        right = ( radius + WHEEL_TRACK / 2.0 * 1000.0);

        if( change_direction )
        {
            vel.left   = (desiredSpeed*right)/right;
            vel.right  = (desiredSpeed*left)/right;
        }
        else
        {
            vel.left   = (desiredSpeed*left)/right;
            vel.right  = (desiredSpeed*right)/right;            
        }
    }
    vel.left = (long)rpmToValue(linearToRpm((double)(vel.left)));
    vel.right = (long)rpmToValue(linearToRpm((double)(vel.right)));
    // Serial.print("left: ");
    // Serial.println(vel.left);
    // Serial.print("right: ");
    // Serial.println(vel.right);    
}

uint8_t buff1[] = {1, 6, 0, 67, 3, 232, 120, 160};
uint8_t buff2[] = {2, 6, 0, 67, 3, 232, 120, 147};

uint16_t calcCRC(uint8_t *buff, uint8_t u8length)
{
    unsigned int temp, temp2, flag;
    temp = 0xFFFF;
    for (unsigned char i = 0; i < u8length; i++)
    {
        temp = temp ^ buff[i];
        for (unsigned char j = 1; j <= 8; j++)
        {
            flag = temp & 0x0001;
            temp >>=1;
            if (flag)
                temp ^= 0xA001;
        }
    }
    // Reverse byte order.
    temp2 = temp >> 8;
    temp = (temp << 8) | temp2;
    temp &= 0xFFFF;
    // the returned value is already swapped
    // crcLo byte is first & crcHi byte is last
    return temp;
}

DiffVel diffDriver(const long speed, const long radius)
{
    static DiffVel diff_vel;
    static uint16_t crc;
    //RPM rpm;

    update(speed, radius, diff_vel);

    // buff1[4] = diff_vel.left >> 8;
    // buff1[5] = diff_vel.left & 0x00ff;
    // crc = calcCRC(buff1, 6);
    // buff1[6] = crc >> 8;
    // buff1[7] = crc & 0x00ff;

    diff_vel.left = -(diff_vel.left);
    // diff_vel.right = -(diff_vel.right);

    // buff2[4] = diff_vel.right >> 8;
    // buff2[5] = diff_vel.right & 0x00ff;
    // crc = calcCRC(buff2, 6);
    // buff2[6] = crc >> 8;
    // buff2[7] = crc & 0x00ff;

    // Serial2.write(buff1, sizeof(buff1));
    // Serial2.flush();
    // Serial3.write(buff2, sizeof(buff2));
    // Serial3.flush();

    return diff_vel;
}
